#include "pid_autotune.h"
#include <sys/time.h>
#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>

static long millis(void)
{
	struct timeval tv;
	gettimeofday(&tv, NULL);
	return (((long)tv.tv_sec) * 1000) + (tv.tv_usec / 1000);
}
pid_autotune_t *pid_autotune_create(float *input, float *output)
{
	pid_autotune_t *handle = (pid_autotune_t *)malloc(sizeof(pid_autotune_t));
	handle->input = input;
	handle->output = output;
	handle->controlType = TYPE_PID; // default to PI
	handle->noiseBand = 0.5;
	handle->running = false;
	handle->oStep = 30;
	pat_set_lookback_sec(handle, 10);
	handle->lastTime = millis();
	return handle;
}
void pid_autotune_free(pid_autotune_t *handle)
{
	if (NULL != handle)
	{
		free(handle);
		handle = NULL;
	}
}

pid_autotune_state pat_runtime(pid_autotune_t *handle)
{
	handle->justevaled = false;
	if (handle->peakCount > 9 && handle->running)
	{
		handle->running = false;
		pat_finish_up(handle);
		return AUTOTUNE_FINISHED;
	}
	unsigned long now = millis();
	printf("now: %lu,lasetime:%lu,sampletime:%d\n", now, handle->lastTime, handle->sampleTime);
	if ((now - handle->lastTime) < handle->sampleTime)
		return false;
	handle->lastTime = now;
	float refVal = *(handle->input);
	handle->justevaled = true;
	if (!handle->running)
	{ // initialize working variables the first time around
		handle->peakType = 0;
		handle->peakCount = 0;
		handle->justchanged = false;
		handle->absMax = refVal;
		handle->absMin = refVal;
		handle->setpoint = refVal;
		handle->running = true;
		handle->outputStart = *(handle->output);
		*(handle->output) = handle->outputStart + handle->oStep;
	}
	else
	{
		if (refVal > handle->absMax)
			handle->absMax = refVal;
		if (refVal < handle->absMin)
			handle->absMin = refVal;
	}

	// oscillate the output base on the input's relation to the setpoint

	if (refVal > handle->setpoint + handle->noiseBand)
		*(handle->output) = handle->outputStart - handle->oStep;
	else if (refVal < handle->setpoint - handle->noiseBand)
		*(handle->output) = handle->outputStart + handle->oStep;

	printf("ref: %f,input:%f,output:%f", refVal, *(handle->input), *(handle->output));

	// bool isMax=true, isMin=true;
	handle->isMax = true;
	handle->isMin = true;
	// id peaks
	for (int i = handle->nLookBack - 1; i >= 0; i--)
	{
		double val = handle->lastInputs[i];
		if (handle->isMax)
			handle->isMax = refVal > val;
		if (handle->isMin)
			handle->isMin = refVal < val;
		handle->lastInputs[i + 1] = handle->lastInputs[i];
	}
	handle->lastInputs[0] = refVal;
	if (handle->nLookBack < 9)
	{ // we don't want to trust the maxes or mins until the inputs array has been filled
		return AUTOTUNE_UNFINISHED;
	}

	if (handle->isMax)
	{
		if (handle->peakType == 0)
			handle->peakType = 1;
		if (handle->peakType == -1)
		{
			handle->peakType = 1;
			handle->justchanged = true;
			handle->peak2 = handle->peak1;
		}
		handle->peak1 = now;
		handle->peaks[handle->peakCount] = refVal;
	}
	else if (handle->isMin)
	{
		if (handle->peakType == 0)
			handle->peakType = -1;
		if (handle->peakType == 1)
		{
			handle->peakType = -1;
			handle->peakCount++;
			handle->justchanged = true;
		}

		if (handle->peakCount < 10)
			handle->peaks[handle->peakCount] = refVal;
	}

	if (handle->justchanged && handle->peakCount > 2)
	{ // we've transitioned.  check if we can autotune based on the last peaks
		double avgSeparation = (abs(handle->peaks[handle->peakCount - 1] - handle->peaks[handle->peakCount - 2]) + abs(handle->peaks[handle->peakCount - 2] - handle->peaks[handle->peakCount - 3])) / 2;
		if (avgSeparation < 0.05 * (handle->absMax - handle->absMin))
		{
			pat_finish_up(handle);
			handle->running = false;
			return AUTOTUNE_FINISHED;
		}
	}
	handle->justchanged = false;
	return AUTOTUNE_UNFINISHED;
}
void pat_cancel(pid_autotune_t *handle)
{
	handle->running = false;
}
void pat_finish_up(pid_autotune_t *handle)
{
	*(handle->output) = handle->outputStart;
	// we can generate tuning parameters!
	handle->Ku = 4 * (2 * handle->oStep) / ((handle->absMax - handle->absMin) * 3.14159);
	handle->Pu = (float)(handle->peak1 - handle->peak2) / 1000;
}
void pat_set_output_step(pid_autotune_t *handle, float step)
{
	handle->oStep = step;
}
float pat_get_output_step(pid_autotune_t *handle)
{
	return handle->oStep;
}

void pat_set_control_type(pid_autotune_t *handle, pid_type_e type)
{
	handle->controlType = type;
}
pid_type_e pat_get_control_type(pid_autotune_t *handle)
{
	return handle->controlType;
}

void pat_set_lookback_sec(pid_autotune_t *handle, int value)
{
	if (value < 1)
		value = 1;

	if (value < 25)
	{
		handle->nLookBack = value * 4;
		handle->sampleTime = 250;
	}
	else
	{
		handle->nLookBack = 100;
		handle->sampleTime = value * 10;
	}
}
int pat_get_lookback_sec(pid_autotune_t *handle)
{
	return handle->nLookBack * handle->sampleTime / 1000;
}

void pat_set_noise_band(pid_autotune_t *handle, float noise)
{
	handle->noiseBand = noise;
}
float pat_get_noise_band(pid_autotune_t *handle)
{
	return handle->noiseBand;
}

float pat_get_kp(pid_autotune_t *handle)
{
	return handle->controlType == 1 ? 0.6 * handle->Ku : 0.4 * handle->Ku;
}

float pat_get_ki(pid_autotune_t *handle)
{
	return handle->controlType == 1 ? 1.2 * handle->Ku / handle->Pu : 0.48 * handle->Ku / handle->Pu; // Ki = Kc/Ti
}
float pat_get_kd(pid_autotune_t *handle)
{
	return handle->controlType == 1 ? 0.075 * handle->Ku * handle->Pu : 0; // Kd = Kc * Td
}
